/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-07     Hao       the first version
 */
#include <hardware/mt6701.h>
#include "drv_common.h"

// 选择使用什么接口, spi为带片选的spi2，i2c为i2c1
#define USE_SPI     0
#define USE_I2C     0

#if USE_I2C
static struct rt_i2c_bus_device *i2c_bus = RT_NULL;
#define MT6701_ADDR         0x06 << 1   // MT6701地址
#define MT6701_REG_ANGLE    0x03        // 寄存器地址
#define MT6701_CPR          16384
#elif USE_SPI
static struct rt_spi_device *spi_mt6701 = RT_NULL;
#endif

int mt6701_init(void)
{
#if USE_I2C
    i2c_bus = (struct rt_i2c_bus_device *) rt_device_find("i2c1");
    RT_ASSERT(i2c_bus != RT_NULL);
#elif USE_SPI
    rt_hw_spi_device_attach(spi_mt6701, "spi2", "spi_mt6701", GET_PIN(B, 12), RT_NULL);
    RT_ASSERT(spi_mt6701 != RT_NULL);

    /* 配置SPI参数 */
    struct rt_spi_configuration cfg;
    cfg.max_hz = 15 * 1000 * 1000;                          /* 根据MT6701的要求设置SPI速率,手册介绍最大速度15MHz */
    cfg.mode = RT_SPI_MASTER | RT_SPI_MODE_0 | RT_SPI_MSB;  /* 根据MT6701的要求设置SPI模式 */
    cfg.data_width = 8;                                     /* 每次传输的位数 */
    rt_spi_configure(spi_mt6701, &cfg);
#endif

    return 0;
}
INIT_APP_EXPORT(mt6701_init);

float mt6701_getAngle()
{
#if USE_I2C
    uint8_t temp[2] = { 0, 0 };
    // 发送命令
    rt_uint8_t buf = MT6701_REG_ANGLE;
    struct rt_i2c_msg msgs;
    msgs.addr = MT6701_ADDR;
    msgs.flags = RT_I2C_WR;
    msgs.buf = &buf;
    msgs.len = 1;
    rt_i2c_transfer(i2c_bus, &msgs, 1);

    // 读取数据
    msgs.addr = MT6701_ADDR;
    msgs.flags = RT_I2C_RD;
    msgs.buf = temp;
    msgs.len = 2;
    if (rt_i2c_transfer(i2c_bus, &msgs, 1) != 1)
    {
        return 0;
    }

    return ((float) ((temp[0] << 6) | (temp[1] >> 2)) * 0.0003834951969f); // angle / 16384 * 6.28318530718f;
#elif USE_SPI
    uint8_t back1, back2, back3;
    uint8_t send_data = 0;

    rt_spi_send_then_recv(spi_mt6701, &send_data, 1, &back1, 1);
    rt_spi_send_then_recv(spi_mt6701, &send_data, 1, &back2, 1);
    rt_spi_send_then_recv(spi_mt6701, &send_data, 1, &back3, 1);

    return (float)(((back1 << 16) | (back2 << 8) | back3) >> 10) * 0.0003834951969f; // angle / 16384 * 6.28318530718f;
#endif

    return 0;
}

#ifdef RT_USING_CONSOLE
#if 0

void mt6701_test()
{
    for (uint8_t i = 0; i <= 50; i++)
    {
        rt_kprintf("&mt6701 = %f\n", mt6701_getAngle());

        rt_thread_mdelay(200);
    }
}
MSH_CMD_EXPORT(mt6701_test, mt6701 test);

#endif
#endif
